#ifndef SERVO_H_
#define SERVO_H_

#include <Servo.h>

#include <config.h>
#include <servo_config.h>
#include "vertify.h"

Servo arm_shoul_pan_s;
Servo arm_shoul_ele_s;
Servo arm_elbow_s;
Servo arm_wrist_s;
Servo arm_grip_s;
Servo arm_twist_s;

Servo mast_pan_s;
Servo mast_ele_s;

Servo steer_fr;
Servo steer_fl;
Servo steer_rr;
Servo steer_rl;

int last[4];

int servo_init(){
  arm_shoul_pan_s.attach(ARM_SHOUL_PAN_PIN);
  arm_shoul_pan_s.writeMicroseconds(ARM_SHOUL_PAN_DEFAULT);

  arm_shoul_ele_s.attach(ARM_SHOUL_ELE_PIN);
  arm_shoul_ele_s.writeMicroseconds(ARM_SHOUL_ELE_DEFAULT);

  arm_elbow_s.attach(ARM_ELBOW_PIN);
  arm_elbow_s.writeMicroseconds(ARM_ELBOW_DEFAULT);

  arm_wrist_s.attach(ARM_WRIST_PIN);
  arm_wrist_s.writeMicroseconds(ARM_WRIST_DEFAULT);

  arm_grip_s.attach(ARM_GRIP_PIN);
  arm_grip_s.writeMicroseconds(ARM_GRIP_DEFAULT);
  
  arm_twist_s.attach(ARM_TWIST_PIN);
  arm_twist_s.writeMicroseconds(ARM_TWIST_DEFAULT);

  mast_ele_s.attach(MAST_ELE_PIN);
  mast_ele_s.writeMicroseconds(SERVO_DEFAULT);

  mast_pan_s.attach(MAST_PAN_PIN);
  mast_pan_s.writeMicroseconds(SERVO_DEFAULT);

  steer_fr.attach(FR_STEER_PIN);
  steer_fr.writeMicroseconds(SERVO_DEFAULT+FR_JOG);

  steer_fl.attach(FL_STEER_PIN);
  steer_fl.writeMicroseconds(SERVO_DEFAULT+FL_JOG);

  steer_rr.attach(RR_STEER_PIN);
  steer_rr.writeMicroseconds(SERVO_DEFAULT+RR_JOG);

  steer_rl.attach(RL_STEER_PIN);
  steer_rl.writeMicroseconds(SERVO_DEFAULT+RL_JOG);
  
  for(int i = 0 ;i < 4; ++i){
    last[i] = SERVO_DEFAULT;
  }
}


int adjustServo(Servo to_command, int value, int index){
  // Purely for testing
  int diff = 0;
  if(value > 128){
    //    Serial.println("Bump up.");
    diff = -5;
  }else if(value < 128){
    //    Serial.println("Bump down.");
    diff = 5;
  }

  int next = last[index] + diff;
  next = constrain(next, SERVO_MIN, SERVO_MAX);
  //  Serial.println(next);

  last[index] = next;

  to_command.writeMicroseconds(next);
}


int commandServo(Servo to_command, int value){
  // Assuming value is valid
  // 0 < value < 256

  // Calculate the percentage different of the pwm
  double per_diff = (value - SERVO_PWM_DEFAULT)/((double)SERVO_PWM_DEFAULT);

  // Find the difference between max and default, should be 300
  int servo_diff = 300;

  // Compute the percentage of this difference to be added to default
  int ms_diff = servo_diff * per_diff;

  // This is the as described above
  int ms_final = SERVO_DEFAULT + ms_diff;

  if(ms_final < 500 || ms_final > 2500){
    Serial.println("In servos::commandServo, final ms was invalid. Was: " + ms_final);
    return -1;
  }
  
  to_command.writeMicroseconds(ms_final);
  return 0;
}

int commandServoMS(Servo to_command, int value){
  // Assume value is valid
  // 0 - 500
  int ms_final = (value * SERVO_MS_SCALE) + SERVO_MIN;
  
  if(ms_final < SERVO_MIN || ms_final > SERVO_MAX){
    Serial.println("In servo::commandServoMS, final ms was invalid. Was: " + ms_final);
    return -1;
  }
  
  to_command.writeMicroseconds(ms_final);
  return 0;
}


int commandServoMS(Servo to_command, int value, int jog){
  // Assume value is valid
  // 0 - 500
  int ms_final = (value * SERVO_MS_SCALE) + SERVO_MIN;
  
  if(ms_final < SERVO_MIN || ms_final > SERVO_MAX){
    Serial.println("In servo::commandServoMS, final ms was invalid. Was: " + ms_final);
    return -1;
  }

  ms_final += jog;
  
  to_command.writeMicroseconds(ms_final);
  return 0;
}


/*int adjustArm(char* comps){
  int shoul_pan = (int)ARM_SHOUL_PAN_MOTOR;
  int shoul_ele = (int)ARM_SHOUL_ELE_MOTOR;
  int elbow = (int)ARM_ELBOW_MOTOR;
  int wrist = (int)ARM_WRIST_MOTOR;

  int shoul_pan_val = getValue(comps, shoul_pan);
  int shoul_ele_val = getValue(comps, shoul_ele);
  int elbow_val = getValue(comps, elbow);
  int wrist_val = getValue(comps, wrist);

  int numErrors = 0;

  if(isValid(shoul_pan_val)){
    if(adjustServo(arm_shoul_pan_s, shoul_pan_val, 0) < 0){
      Serial.println("Unable to send valud to arm shoul pan motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid shoulder pan value.");
    numErrors++;
  }

  if(isValid(shoul_ele_val)){
    if(adjustServo(arm_shoul_ele_s, shoul_ele_val, 1) < 0){
      Serial.println("Unable to send value to arm shoul ele motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid arm shoul ele motor value.");
    numErrors++;
  }

  if(isValid(elbow_val)){
    if(adjustServo(arm_elbow_s, elbow_val, 2) < 0){
      Serial.println("Unable to send vale to arm elbow motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid arm elbow motor value.");
    numErrors++;
  }

  if(isValid(wrist_val)){
    if(adjustServo(arm_wrist_s, wrist_val, 3) < 0){
      Serial.println("Unable to send value to arm wrist motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid arm wrist motor value.");
    numErrors++;
    }

  if(numErrors > 0){
    Serial.println("Errors occured in command arm servos. Num: " + numErrors);
    return -1;
  }
  return 0;
}*/

int commandGripper(int grip_val){
  if(grip_val > 128){
    //Grip
    arm_grip_s.writeMicroseconds(2000);
  }else{
    //relax
    arm_grip_s.writeMicroseconds(1150);
  }
  return 0;
}

int commandTwist(int twist_val){
  // Assuming value is valid
  // 0 < value < 256

  // Calculate the percentage different of the pwm
  double per_diff = (twist_val - SERVO_PWM_DEFAULT)/((double)SERVO_PWM_DEFAULT);

  // Find the difference between max and default, should be 300
  int servo_diff = 900;

  // Compute the percentage of this difference to be added to default
  int ms_diff = servo_diff * per_diff;

  // This is the as described above
  int ms_final = SERVO_DEFAULT + ms_diff;

  if(ms_final < 500 || ms_final > 2500){
    Serial.println("In servos::commandServo, final ms was invalid. Was: " + ms_final);
    return -1;
  }

  arm_twist_s.writeMicroseconds(ms_final);
  return 0;
}

int commandArm(char* comps){
  int shoul_pan_0 = (int)ARM_SHOUL_PAN_MOTOR_0;
  int shoul_pan_1 = (int)ARM_SHOUL_PAN_MOTOR_1;

  int shoul_ele_0 = (int)ARM_SHOUL_ELE_MOTOR_0;
  int shoul_ele_1 = (int)ARM_SHOUL_ELE_MOTOR_1;

  int elbow_0 = (int)ARM_ELBOW_MOTOR_0;
  int elbow_1 = (int)ARM_ELBOW_MOTOR_1;

  int wrist_0 = (int)ARM_WRIST_MOTOR_0;
  int wrist_1 = (int)ARM_WRIST_MOTOR_1;

  int grip_0 = (int)ARM_GRIP_MOTOR_0;
  int grip_1 = (int)ARM_GRIP_MOTOR_1;

  int twist_0 = (int)ARM_TWIST_MOTOR_0;
  int twist_1 = (int)ARM_TWIST_MOTOR_1;

  int shoul_pan_0_val = getValue(comps, shoul_pan_0);
  int shoul_pan_1_val = getValue(comps, shoul_pan_1);

  int shoul_ele_0_val = getValue(comps, shoul_ele_0);
  int shoul_ele_1_val = getValue(comps, shoul_ele_1);

  int elbow_0_val = getValue(comps, elbow_0);
  int elbow_1_val = getValue(comps, elbow_1);

  int wrist_0_val = getValue(comps, wrist_0);
  int wrist_1_val = getValue(comps, wrist_1);

  int grip_0_val = getValue(comps, grip_0);
  int grip_1_val = getValue(comps, grip_1);

  int twist_0_val = getValue(comps, twist_0);
  int twist_1_val = getValue(comps, twist_1);

  int shoul_pan_val = shoul_pan_0_val + shoul_pan_1_val;
  int shoul_ele_val = shoul_ele_0_val + shoul_ele_1_val;
  int elbow_val = elbow_0_val + elbow_1_val;
  int wrist_val = wrist_0_val + wrist_1_val;
  int grip_val = grip_0_val + grip_1_val;
  int twist_val = twist_0_val + twist_1_val;

  int numErrors = 0;

  if(isValidMs(shoul_pan_val)){
    if(commandServoMS(arm_shoul_pan_s, shoul_pan_val) < 0){
      Serial.println("Unable to send valud to arm shoul pan motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid shoulder pan value.");
    numErrors++;
  }

  if(isValidMs(shoul_ele_val)){
    if(commandServoMS(arm_shoul_ele_s, shoul_ele_val) < 0){
      Serial.println("Unable to send value to arm shoul ele motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid arm shoul ele motor value.");
    numErrors++;
  }

  if(isValidMs(elbow_val)){
    if(commandServoMS(arm_elbow_s, elbow_val) < 0){
      Serial.println("Unable to send vale to arm elbow motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid arm elbow motor value.");
    numErrors++;
  }

  if(isValidMs(wrist_val)){
    if(commandServoMS(arm_wrist_s, wrist_val) < 0){
      Serial.println("Unable to send value to arm wrist motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid arm wrist motor value.");
    numErrors++;
  }

  if(isValidMs(grip_val)){
    if(commandServoMS(arm_grip_s, grip_val) < 0){
      Serial.println("Unable to send value to grip motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid arm grip motor value.");
    numErrors++;
  }

  if(isValidMs(twist_val)){
    if(commandServoMS(arm_twist_s, twist_val) < 0){
      Serial.println("Unable to send value to twist motor.");
      numErrors++;
    }
  }else{
    Serial.println("Inavlid twist motor value.");
    numErrors++;
  }

  if(numErrors > 0){
    Serial.println("Errors occured in command arm servos. Num: " + numErrors);
    return -1;
  }
  return 0;
}

int commandMast(char* comps){
  int mast_ele_0 = (int)MAST_ELE_MOTOR_0;
  int mast_ele_1 = (int)MAST_ELE_MOTOR_1;
  
  int mast_pan_0 = (int)MAST_PAN_MOTOR_0;
  int mast_pan_1 = (int)MAST_PAN_MOTOR_1;

  int mast_ele_0_value = getValue(comps, mast_ele_0);
  int mast_ele_1_value = getValue(comps, mast_ele_1);

  int mast_pan_0_value = getValue(comps, mast_pan_0);
  int mast_pan_1_value = getValue(comps, mast_pan_1);

  int mast_pan_value = mast_pan_0_value + mast_pan_1_value;
  int mast_ele_value = mast_ele_0_value + mast_ele_1_value;

  int numErrors = 0;

  if(isValidMs(mast_pan_value)){
    if(commandServoMS(mast_pan_s, mast_pan_value) < 0){
      Serial.println("Unable to send value to mast pan motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid mast pan value.");
    numErrors++;
  }

  if(isValidMs(mast_ele_value)){
    if(commandServoMS(mast_ele_s, mast_ele_value) < 0){
      Serial.println("Unable to send value to mast ele motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid mast ele value.");
    numErrors++;
  }

  if(numErrors > 0){
    Serial.println("Errors occured in mast servos. Num: " + numErrors);
    return -1;
  }
  return 0;
}

int commandSteering(char* comps){
  int fr_0 = (int)FR_S_MOTOR_0;
  int fr_1 = (int)FR_S_MOTOR_1;

  int fl_0 = (int)FL_S_MOTOR_0;
  int fl_1 = (int)FL_S_MOTOR_1;

  int rr_0 = (int)RR_S_MOTOR_0;
  int rr_1 = (int)RR_S_MOTOR_1;

  int rl_0 = (int)RL_S_MOTOR_0;
  int rl_1 = (int)RL_S_MOTOR_1;

  int fr_0_value = getValue(comps, fr_0);
  int fr_1_value = getValue(comps, fr_1);

  int fl_0_value = getValue(comps, fl_0);
  int fl_1_value = getValue(comps, fl_1);

  int rr_0_value = getValue(comps, rr_0);
  int rr_1_value = getValue(comps, rr_1);

  int rl_0_value = getValue(comps, rl_0);
  int rl_1_value = getValue(comps, rl_1);

  int fr_value = fr_0_value + fr_1_value;
  int fl_value = fl_0_value + fl_1_value;
  int rr_value = rr_0_value + rr_1_value;
  int rl_value = rl_0_value + rl_1_value;

  int numErrors = 0;

  if(isValidMs(fr_value)){
    if(commandServoMS(steer_fr, fr_value, FR_JOG) < 0){
      Serial.println("Unable to send value to steer fr motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid steer fr value.");
    numErrors++;
  }

  if(isValidMs(fl_value)){
    if(commandServoMS(steer_fl, fl_value, FL_JOG) < 0){
      Serial.println("Unable to send value to steer fl motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid steer fl vlaue.");
    numErrors++;
  }

  if(isValidMs(rr_value)){
    if(commandServoMS(steer_rr, rr_value, RR_JOG) < 0){
      Serial.println("Unable to send value to steer rr motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid steer rr value.");
    numErrors++;
  }

  if(isValidMs(rl_value)){
    if(commandServoMS(steer_rl, rl_value, RL_JOG) < 0){
      Serial.println("Unable to send value to steer rl motor.");
      numErrors++;
    }
  }else{
    Serial.println("Invalid steer rl value.");
    numErrors++;
  }

  if(numErrors > 0){
    Serial.println("Errors occurred in steering servos. Num: " + numErrors);
    return -1;
  }
  return 0;
}

int commandServos(char* comps){
  if(commandArm(comps) < 0){
    Serial.println("Unable to command arm correctly.");
    return -1;
  }

  /*  if(adjustArm(comps) < 0){
    Serial.println("Unable to adjust arm correctly.");
    return -1;
    }*/

  if(commandSteering(comps) < 0){
    Serial.println("Unable to command steering correctly.");
    return -1;
  }

  if(commandMast(comps) < 0){
    Serial.println("Unable to command mast correctly.");
    return -1;
  }

  return 0;
}

#endif
